#include "BPEPhysicalObject.h"

BPEPhysicalObject::BPEPhysicalObject(){

	bpePosition = 0;
	mass = 1;

	bpeForce = BPEVector3d(0,0,0);
	bpeSpeed = BPEVector3d(0,0,0);
	bpeRotationalForce.clear();

	


};

BPEPhysicalObject::BPEPhysicalObject(const vector3df *position , const double &mass){

	bpePosition = position;
	this->mass = mass;

	bpeForce = BPEVector3d(0,0,0);
	bpeSpeed = BPEVector3d(0,0,0);
	BPEVector3d bpeRotationalSpeed= BPEVector3d(0,0,0);

	bpeRotationalForce.clear();


};

double BPEPhysicalObject::getMass(){	return mass;	};
void BPEPhysicalObject::setMass(const double &m){	mass = m;	};

BPEPoint3d BPEPhysicalObject::getPosition(){	return BPEPoint3d(bpePosition->X,bpePosition->Y,bpePosition->Z);	};




void BPEPhysicalObject::resetAllForces(){
	resetMovementForces();
	resetRotationalForces();
}

void BPEPhysicalObject::resetMovementForces(){
	bpeForce = BPEVector3d(0,0,0);
};

void BPEPhysicalObject::addForce(const BPEVector3d &f){
	bpeForce.add(f);
};

void BPEPhysicalObject::resetRotationalForces(){
	bpeRotationalForce.clear();
};

void BPEPhysicalObject::addRotationalForce(const BPEVector3d &f){
	bpeRotationalForce.push_back(f);
};

void BPEPhysicalObject::calculateSpeed(){	

	bpeSpeed.moveTo(this->getPosition());


	BPEVector3d acceleration(this->bpeForce.getTail(),
		this->bpeForce.getXYRads(), 
		this->bpeForce.getXZRads(), 
		this->bpeForce.getMagnitude()/this->mass);

	this->bpeSpeed.add(acceleration);

};


void BPEPhysicalObject::calculateRotationalSpeed(){

	int size = this->bpeRotationalForce.size();

	//calculate torque = roph
	//i need the magnitude and the angle of the vector as well as the distance of this vector to a point(center of the physical object)
	BPEVector3d torque(bpeRotationalForce[0].getTail(),
		bpeRotationalForce[0].getXYRads(),	bpeRotationalForce[0].getXZRads(), 
		bpeRotationalForce[0].getMagnitude()/ bpeRotationalForce[0].distanceFromAPoint(this->getPosition()));
	

	for(int i = 1; i< size; i++){

		BPEVector3d torquePart(bpeRotationalForce[i].getTail(),
			bpeRotationalForce[i].getXYRads(),	bpeRotationalForce[i].getXZRads(), 
			bpeRotationalForce[i].getMagnitude()/ bpeRotationalForce[i].distanceFromAPoint(this->getPosition()) );
	
		torque.add(torquePart);

	}

	//we accept the the object(or the is a compact sphere
	//so the Moment of inertia(roph adraneias) = 2/5*m*R^2
	//but i am going to use a fixed number for now...
	//lets just say momentOfInertia = ...
	double momentOfInertia = 20;

	//so th rotational acceleration is a = torque / momentOfInertia
	BPEVector3d rAcceleration(torque.getTail(),
		torque.getXYRads(), torque.getXZRads(),
		torque.getMagnitude() / momentOfInertia);

	bpeRotationalSpeed.add(rAcceleration);
};


void BPEPhysicalObject::setSpeed(const BPEVector3d &vec)	{	bpeSpeed = vec;	};
void BPEPhysicalObject::setRotationalSpeed(const BPEVector3d &vec)	{	bpeRotationalSpeed = vec;	};
